#include "stdafx.h"
#include "Anim2Dyn.h"

#include "glm.hpp"
#include "ext.hpp"

#include <ctime>

#include "GlobalAppData.h"

#include "Scene.h"
#include "Node.h"
#include "ModelNode.h"
#include "WorldData.h"
#include "NodeUtil.h"

#include "PointMass.h"

using namespace ani;

struct PointMassObject
{
	PointMass Point;
	mscene::Node * Object;
	void setObjectPosition()
	{
		Object->Position = *(XMFLOAT3*)&Point.mPosition;
	}
};

static PointMassObject sObj1;
static float sTime = 0.0f;
static float sSpeed = 100.0f;

static mscene::Node * sForcePointer = NULL;
static mscene::Node * sCircleCenter = NULL;

glm::vec3 sCirclePos = glm::vec3(-100, 0, 0);

void Anim2Dyn::init( mscene::Scene * scene, mgfx::RenderingSystem * rendSys, mphys::PhysicsSystem * physSys )
{
	AnimView::init(scene, rendSys, physSys);

	mscene::PrimitiveNodeDesc objDesc;
	objDesc.IsStatic = true;
	objDesc.Scale = XMFLOAT3(0.2f, 0.2f, 0.2f);
	sObj1.Object = mscene::createSphereNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));
	sObj1.Point = PointMass(glm::vec3(0,0,0), 1.0f);
	sObj1.Point.mVelocity = glm::vec3(0, 0, 0);

	objDesc.Scale = XMFLOAT3(0.04f, 0.04f, 0.04f);
	sForcePointer = mscene::createBoxNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));

	objDesc.Scale = XMFLOAT3(0.1f, 0.1f, 0.1f);
	sCircleCenter = mscene::createSphereNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));

	initGui();
}

static inline XMFLOAT3 toFloat3(XMVECTOR vec)
{
	XMFLOAT3 res;
	XMStoreFloat3(&res, vec);
	return res;
}

void Anim2Dyn::update(float dt)
{ 	
	sTime += dt;

	// circular movement
	float scaledTime = sTime * 2; // T = PI
	
	/*
	float circleRadius = sSpeed * PI / (2 * PI); // v = 2*PI*r/T
	float forceStr = sObj1.Point.mMass * sSpeed * sSpeed / circleRadius;
	float forceDirX = cosf(scaledTime);
	float forceDirZ = sinf(scaledTime);
	glm::vec3 forceDir = -glm::vec3(forceDirX, 0, forceDirZ);
	sObj1.Point.addForce(forceDir * forceStr);

	sCircleCenter->Position = XMFLOAT3(-circleRadius, 0, 0);
	*/

	float velocityValue = glm::length(sObj1.Point.mVelocity);
	if(velocityValue > 0)
	{
		float circleRadius = glm::length(sCirclePos - sObj1.Point.mPosition);
		float forceStr = sObj1.Point.mMass * velocityValue * velocityValue / circleRadius;
		glm::vec3 forceDir = glm::normalize( sCirclePos - sObj1.Point.mPosition );
		sObj1.Point.addForce(forceDir * forceStr);
	}

	sCircleCenter->Position = *(XMFLOAT3*)&sCirclePos;

	//glm::vec3 forceDir = glm::vec3(-sObj1.Point.mVelocity.z, 0, sObj1.Point.mVelocity.x);
	//float velLength = glm::length(sObj1.Point.mVelocity);
	//float circleRadius = 1000.0f;
	//float forceStr = sObj1.Point.mMass * sSpeed * sSpeed / circleRadius;
	//sObj1.Point.addForce(forceDir * forceStr);

	sObj1.Point.integrate(dt);
	sObj1.setObjectPosition();

	
	LOG_STREAM("x: " << sObj1.Point.mPosition.x << ", z: " << sObj1.Point.mPosition.z);

	//sForcePointer->Scale.x = 0.04f + forceStr * 0.006f;
	//glm::vec3 pointerPos = sObj1.Point.mPosition + forceDir * sForcePointer->Scale.x * 18.0f;
	//sForcePointer->Position = *(XMFLOAT3*)&pointerPos;
	//XMStoreFloat4(&sForcePointer->Orientation, XMQuaternionRotationRollPitchYaw(0, -scaledTime, 0));
	
	
	
	AnimView::update(dt);
}




void TW_CALL start(void *clientData)
{
	sObj1.Point.addForce(glm::vec3(0, 0, 10000));
}

void Anim2Dyn::initGui()
{
	TwBar * animBar = TwNewBar("Point");
	TwAddButton(animBar, "Start", start, NULL, "");
}
